#include "ConstraintCircle.h"

ConstraintCircle::ConstraintCircle(void)
{
}

ConstraintCircle::~ConstraintCircle(void)
{
}

void ConstraintCircle::applyConstraint(Particle& p, float x, float y, float Ks, float Kd)
{
	double C  = 0.5*( dot(p.getPosition(), p.getPosition() ) - (25.0/2.0) );	
	double dC = dot( p.getPosition(), p.getVelocity() );
	
	double l = ( dot(-p.getForce(), p.getPosition()) - ( dot(p.getMass()*p.getVelocity(), p.getVelocity()) ) - (Ks*C) - (Kd*dC) ) 
		/ dot(p.getPosition(), p.getPosition());
	
	Vec3f f(l*p.getPosition());
	p.addForce(f[0], f[1], f[2]);
}
